
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.DigitalIOButton;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.templates.assets.shooterstates.ZingStateMachine;
import edu.wpi.first.wpilibj.templates.commands.ActivateIntake;
import edu.wpi.first.wpilibj.templates.commands.ActivateShifter;
import edu.wpi.first.wpilibj.templates.commands.AutonMode;
import edu.wpi.first.wpilibj.templates.commands.ChangeIndexerState;
import edu.wpi.first.wpilibj.templates.commands.DasiesIngest;
import edu.wpi.first.wpilibj.templates.commands.DasiesReverse;
import edu.wpi.first.wpilibj.templates.commands.DigitalScale;
import edu.wpi.first.wpilibj.templates.commands.MagicalStateMachine;
import edu.wpi.first.wpilibj.templates.commands.SetArmPosition;
import edu.wpi.first.wpilibj.templates.commands.Shoot;
import edu.wpi.first.wpilibj.templates.commands.ShootReverse;
import edu.wpi.first.wpilibj.templates.commands.StopShooter;
import edu.wpi.first.wpilibj.templates.commands.TopIndexerForward;
import edu.wpi.first.wpilibj.templates.commands.TopIndexerReverse;
import edu.wpi.first.wpilibj.templates.commands.Track;
import edu.wpi.first.wpilibj.templates.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.ArmSubsystem.PresetArmPosition;
import edu.wpi.first.wpilibj.templates.subsystems.DifferentialSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.DifferentialSubsystem.DifferentialState;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    
    /*
     * Global Class Variables
     */
    LogitechController driverController;
    LogitechController coDriverController;
    LogitechController cal;
    
    LogitechControllerButton driverA;
    LogitechControllerButton driverB;
    LogitechControllerButton driverX;
    LogitechControllerButton driverY;
    LogitechControllerButton driverLeftBumper;
    LogitechControllerButton driverLeftTrigger;
    LogitechControllerButton driverRightBumper;
    LogitechControllerButton driverRightTrigger;
    LogitechControllerButton driverStart;
    LogitechControllerButton driverBack;
    
    LogitechControllerButton coDriverA;
    LogitechControllerButton coDriverB;
    LogitechControllerButton coDriverX;
    LogitechControllerButton coDriverY;
    LogitechControllerButton coDriverLeftBumper;
    LogitechControllerButton coDriverLeftTrigger;
    LogitechControllerButton coDriverRightBumper;
    LogitechControllerButton coDriverRightTrigger;
    LogitechControllerButton coDriverStart;
    LogitechControllerButton coDriverBack;
    
    LogitechControllerButton calA;
    
    /*
     * PLAN B CONTROLS
     * 
     * Only for the extreme scenario where all of our main controllers have
     * been trashed and we have no alternative.
     * 
     * Or for someone that absolutely hates Logitech Controllers.
     */
//    Joystick leftStick;
//    
//    JoystickButton leftStickTrigger;
//    JoystickButton leftStickTwo;
//    JoystickButton leftStickThree;
//    
//    Joystick righStick;
//    
//    JoystickButton rightStickTrigger;
//    JoystickButton rightStickTwo;
//    JoystickButton rightStickThree;
    
    
    // CONSTRUCTOR //
    
    
    public OI() {
        
        driverController = new LogitechController(1);
            driverA = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonA.value);
            driverB = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonB.value);
            driverX = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonX.value);
            driverY = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonY.value);
            driverLeftBumper = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonLeftB.value);
            driverLeftTrigger = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonLeftTrigger.value);
            driverRightBumper = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonRightB.value);
            driverRightTrigger = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonRightTrigger.value);
            driverStart = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonStart.value);
            driverBack = new LogitechControllerButton(driverController, LogitechController.ButtonType.kButtonBack.value);
            
            driverLeftBumper.whenPressed(new ActivateShifter(DifferentialState.LOW_GEAR));
            driverLeftTrigger.whenPressed(new ActivateShifter(DifferentialState.HIGH_GEAR));
            driverA.whenPressed(new DigitalScale(DigitalScale.HALF_SCALE));
            driverY.whenPressed(new DigitalScale(DigitalScale.FULL_SCALE));
            driverRightTrigger.whileHeld(new ShootReverse());
            driverRightBumper.whileHeld(new Shoot());
            
        
        coDriverController = new LogitechController(2);
            coDriverA = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonA.value);
            coDriverB = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonB.value);
            coDriverX = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonX.value);
            coDriverY = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonY.value);
            coDriverLeftBumper = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonLeftB.value);
            coDriverLeftTrigger = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonLeftTrigger.value);
            coDriverRightBumper = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonRightB.value);
            coDriverRightTrigger = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonRightTrigger.value);
            coDriverStart = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonStart.value);
            coDriverBack = new LogitechControllerButton(coDriverController, LogitechController.ButtonType.kButtonBack.value);

            coDriverLeftTrigger.whileHeld(new DasiesIngest());
            coDriverLeftBumper.whileHeld(new DasiesReverse());
        
            coDriverA.whileHeld(new MagicalStateMachine());
     
//            coDriverRightBumper.whenPressed(new Track());
//            coDriverRightBumper.whenReleased(new StopShooter());
            coDriverRightBumper.whileHeld(new TopIndexerReverse());
            coDriverRightTrigger.whileHeld(new TopIndexerForward());
//            coDriverRightTrigger.whileHeld(new Shoot());
            
            coDriverStart.whileHeld(new Shoot());
            coDriverBack.whileHeld(new ShootReverse());
            
        cal = new LogitechController(3);
            calA = new LogitechControllerButton(cal, LogitechController.ButtonType.kButtonA.value);
            
            calA.whenPressed(new SetArmPosition(PresetArmPosition.IN_BOX));

        
        /*
         * PLAN B!
         */
        
            
    } // end OI
    
    /**
     * Gets the Driver's controller for other files to interface with.
     * 
     * @return the Driver's controller.
     */
    
    public LogitechController getDriverController() {
        return driverController;
    } // end getDriverController
    
    /**
     * Gets the Co-Driver's controller for other files to interface with.
     * 
     * @return The Co-Driver's controller.
     */
    
    public LogitechController getCoDriverController() {
        return coDriverController;
    } // end getCoDriverController
    
} // end IO

